#!/usr/bin/env python
import roslib; roslib.load_manifest('EventPlanner')

import rospy
import actionlib

from EventPlanner.msg import MoveHeadStatus
from actionlib_msgs.msg import GoalStatus
from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal
from EventPlanner.msg import MoveHeadServerGoal

class MoveHeadServer:
	def __init__(self):
		self.running = False
		statedInitializeInfo = False
		self.headClient = actionlib.SimpleActionClient(
					'/head_traj_controller/point_head_action', PointHeadAction)
		rospy.logwarn("Initializing Move Head Server")
		while self.headClient.wait_for_server(rospy.Duration(1.0)):
			if not statedInitializeInfo:
				rospy.loginfo("Waiting for Head Client To Initialize...")
				statedInitializeInfo = True
			rospy.sleep(.5)

		#sub = rospy.Subscriber("moveHead_cancelPrevious", PointHeadGoal,self.moveHeadCallbackCancelPrevious)
		#sub2 = rospy.Subscriber("moveHead", PointHeadGoal, self.moveHeadCallback)
		sub = rospy.Subscriber("moveHead", MoveHeadServerGoal, self.moveHeadCallback)

		self.publisher = rospy.Publisher("moveHeadFinished",MoveHeadStatus);
		rospy.logwarn("Move Head Server Initialized")
		

	def moveHeadCallback(self,newGoal):
		if self.running and ( newGoal.overridePrevious is None or not newGoal.overridePrevious):
			return
		else:
			self.headClient.cancel_goal()
		self.running = True
		self.lookAtPoint(newGoal.goal);

		
	def lookAtPoint(self, headGoal):
		# Point Head
		self.headClient.send_goal(headGoal)
		
		# Say that we are finished
		status = MoveHeadStatus();
		status.finished = True;
		self.publisher.publish(status);

		# Accept new Goals ( assuming we aren't overriding )
		self.running=False;



if __name__=="__main__":
    rospy.init_node("MoveHeadServer")
    server = MoveHeadServer()
    rospy.spin()
